import gym
import torch
import torch.nn.functional as F
from torch.distributions import Normal
import numpy as np
import rl_utils


class PolicyNetContinuous(torch.nn.Module):
    def __init__(self, state_dim, hidden_dim, action_dim):
        super(PolicyNetContinuous, self).__init__()
        self.fc1 = torch.nn.Linear(state_dim, hidden_dim[0])
        self.fc2 = torch.nn.Linear(hidden_dim[0], hidden_dim[1])
        self.fc_mu = torch.nn.Linear(hidden_dim[1], action_dim)
        self.fc_std = torch.nn.Linear(hidden_dim[1], action_dim)
        self.fc_mu.weight.data.mul_(0.1)
        self.fc_mu.bias.data.mul_(0.0)

    def forward(self, x):
        x = F.relu(self.fc1(x))
        x = F.relu(self.fc2(x))
        # mu = 2.0 * torch.tanh(self.fc_mu(x))
        mu = torch.tanh(self.fc_mu(x))*0.4 # [-1,1] * 0.4
        std = F.sigmoid(self.fc_std(x))*0.4 # (0,1) * 0.4 要加上一个很小的数！！！！！！
        # std = F.sigmoid(self.fc_std(x))*0.4+1e-6 # (0,1) * 0.4 要加上一个很小的数！！！！！！
        # std = F.softplus(self.fc_std(x)) # (0,)
        
        # x = F.tanh(self.fc1(x))
        # x = F.tanh(self.fc2(x))
        # mu = self.fc_mu(x)
        # log_std = self.fc_std(x)
        # std = torch.exp(log_std)
        return mu, std


class ValueNet(torch.nn.Module):
    def __init__(self, state_dim, hidden_dim):
        super(ValueNet, self).__init__()
        self.fc1 = torch.nn.Linear(state_dim, hidden_dim[0])
        self.fc2 = torch.nn.Linear(hidden_dim[0], hidden_dim[1])
        self.fc3 = torch.nn.Linear(hidden_dim[1], 1)
        self.fc3.weight.data.mul_(0.1)
        self.fc3.bias.data.mul_(0.0)

    def forward(self, x):
        x = F.relu(self.fc1(x))
        x = F.relu(self.fc2(x))
        # x = F.tanh(self.fc1(x))
        # x = F.tanh(self.fc2(x))
        values = self.fc3(x)
        return values

class PPOContinuous:
    ''' 处理连续动作的PPO算法 '''
    def __init__(self, state_dim, hidden_dim, action_dim, actor_lr, critic_lr,
                 para_GAE_lmbda, epochs, para_PPO_clip, discount_fac, action_clip, device):
        self.device = device
        self.epochs = epochs  # 一条序列的数据用来训练轮数
        self.actor = PolicyNetContinuous(state_dim, hidden_dim, action_dim).to(device)
        self.critic = ValueNet(state_dim, hidden_dim).to(device)
        self.actor_optimizer = torch.optim.Adam(self.actor.parameters(), lr=actor_lr)
        self.critic_optimizer = torch.optim.Adam(self.critic.parameters(), lr=critic_lr)
        self.discount_fac = discount_fac
        self.para_GAE_lmbda = para_GAE_lmbda
        self.para_PPO_clip = para_PPO_clip  # PPO中截断范围的参数
        self.action_clip = action_clip
        self.log_probs_min = np.log(1e-9)

    def take_action(self, state):
        state = torch.tensor(state, dtype=torch.float).to(self.device)
        mu, sigma = self.actor(state)
        action = Normal(mu, sigma).sample()
        # action = action.item()
        action = action.detach().numpy().reshape(-1)
        # action = np.clip(action, -self.action_clip, self.action_clip)
        # print('action.shape = ', action.shape) # 一维数组
        # print('action = ', action)
        return action

    def update(self, transition_dict):
        states = torch.tensor(np.array(transition_dict['states']), dtype=torch.float).to(self.device)
        actions = torch.tensor(np.array(transition_dict['actions']), dtype=torch.float).to(self.device)
        rewards = torch.tensor(transition_dict['rewards'], dtype=torch.float).view(-1, 1).to(self.device)
        next_states = torch.tensor(np.array(transition_dict['next_states']), dtype=torch.float).to(self.device)
        dones = torch.tensor(transition_dict['dones'],dtype=torch.float).view(-1, 1).to(self.device)
        # print('states.shape = ', states.shape)
        # states_ = states.detach().numpy()
        # print('len(states[np.isnan(states)]) = ', len(states_[np.isnan(states_)]))
        # print('actions.shape = ', actions.shape)
        # rewards = (rewards + 8.0) / 8.0  # 和TRPO一样,对奖励进行修改,方便训练
        
        td_target = rewards + self.discount_fac * self.critic(next_states) * (1 - dones)
        td_delta = td_target - self.critic(states)
        # 根据策略\theta '的优势
        advantage = rl_utils.compute_advantage(self.discount_fac, self.para_GAE_lmbda, td_delta.cpu()).to(self.device)
        # print('td_delta.shape = ', td_delta.shape)
        # print('advantage.shape = ', advantage.shape)
        # 动作是正态分布
        mu, std = self.actor(states)
        # states 下，根据策略\theta '，采取各个动作的概率的对数
        old_log_probs = Normal(mu.detach(), std.detach()).log_prob(actions)
        old_log_probs = torch.clamp(old_log_probs, self.log_probs_min, None)

        for _ in range(self.epochs):
            # for each minibatch of size B !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
            mu, std = self.actor(states)
            # states 下，根据策略\theta，采取各个动作的概率的对数
            log_probs = Normal(mu, std).log_prob(actions)
            log_probs = torch.clamp(log_probs, self.log_probs_min, None)
            # 采取各个动作的概率的比
            ratio = torch.exp(log_probs - old_log_probs)
            # print('mu.shape = ', mu.shape) # [每个episode有多少次, action_dim]
            # print('std.shape = ', std.shape) # [每个episode有多少次, action_dim]
            # print('log_probs.shape = ', log_probs.shape) # [每个episode有多少次, action_dim]
            # print('ratio.shape = ', ratio.shape) # [每个episode有多少次, action_dim]
            surr2 = torch.clamp(ratio, 1 - self.para_PPO_clip, 1 + self.para_PPO_clip) * advantage  # 截断
            actor_loss = torch.mean(-torch.min(ratio * advantage, surr2))  # PPO损失函数
            # print('torch.min(surr1, surr2).shape = ', torch.min(surr1, surr2).shape) # [每个episode有多少次, action_dim]
            # print('actor_loss.shape = ', actor_loss.shape)
            # print('self.critic(states).shape = ', self.critic(states).shape)
            # hhhhhhhhhhhh
            self.actor_optimizer.zero_grad()
            actor_loss.backward()
            self.actor_optimizer.step()

            critic_loss = torch.mean(F.mse_loss(self.critic(states), td_target.detach()))
            self.critic_optimizer.zero_grad()
            critic_loss.backward()
            self.critic_optimizer.step()
          


actor_lr = 1e-4
critic_lr = 5e-3
num_episodes = 2000
hidden_dim = [128,64]
discount_fac = 0.9
para_GAE_lmbda = 0.9
epochs = 10
para_PPO_clip = 0.2
action_clip = 0.4
device = torch.device("cuda") if torch.cuda.is_available() else torch.device("cpu")

# env_name = 'Pendulum-v1'
env_name = 'Humanoid-v4'
env = gym.make(env_name)
random_seed=20240222
env.reset(seed=random_seed)
np.random.seed(random_seed)
torch.manual_seed(random_seed)
# torch.cuda.manual_seed_all(random_seed)
# torch.backends.cudnn.deterministic = True

state_dim = env.observation_space.shape[0]
action_dim = env.action_space.shape[0]  # 连续动作空间
print('state_dim = ', state_dim)
print('action_dim = ', action_dim)
agent = PPOContinuous(state_dim, hidden_dim, action_dim, actor_lr, critic_lr,para_GAE_lmbda, epochs, para_PPO_clip, discount_fac, action_clip, device)
alg_name = 'PPO'

print('Training!!!!')
# return_list = rl_utils.train_on_policy_agent0125(env, agent, num_episodes)
return_list = rl_utils.train_on_policy_agent(env, agent, num_episodes)

rl_utils.plot_results(return_list, env_name, alg_name, string_train_test = 'Training', moving_average_weight = 9)

print('Testing!!!!')
return_list_test = rl_utils.test_agent(env, agent, num_episodes = 50)
rl_utils.plot_results(return_list_test, env_name, alg_name, string_train_test = 'Testing', moving_average_weight = 3)
print('Rendering!!!!')
rl_utils.test_agent_render(env, agent)